#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>

#include "PinnedObject.h"
#include "include.h"

#define BOLT_FILE "./Images/nut.bmp"


Vector2D PinnedObject::VelocityAtPoint(Vector2D point)
{
	point = point - pinPoint;
	float tangantialVelocity = point.Magnitude() * angularVelocity;
	Vector2D dir = Vector2D(-point.y, point.x);
	if(dir.Magnitude() > 0.0)
	{
		return !dir * tangantialVelocity;
	}
	return Vector2D(0,0);
}
bool PinnedObject::PreThinkEntryPoint(double elapsedTime)
{
	//Add pin friction
	float maxFriction = 1000.0;
	float pinFriction = 0.0;
	if(abs(angularVelocity) < maxFriction)
		pinFriction = -angularVelocity;
	else
	{
		if(angularVelocity < 0)
			pinFriction = maxFriction;
		else
			pinFriction = -maxFriction;
	}
	torque += pinFriction * angularMass;
	return false;
}
void PinnedObject::Rotate(Vector2D radius, Vector2D delta)
{
	if(delta.Magnitude() == 0.0 || radius.Magnitude() == 0.0)
		return;
		
	radius = radius - pinPoint;
	float direction = 1.0;
	if(angularVelocity > 0.0)
		direction = -1.0;
	Rotate(-rotation);//direction * 2 * delta.Magnitude()/(radius.Magnitude()));
}
void PinnedObject::Rotate(float theta)
{
	if(abs(theta) < 0.000001)
		return;
		
	float sinTheta = sin(theta);
	float cosTheta = cos(theta);

	//rotate the center point
	Vector2D center = -1.0 * pinPoint;
	float newX = center.x * cosTheta - center.y * sinTheta;
	float newY = center.y * cosTheta + center.x * sinTheta;
	center.x = newX;
	center.y = newY;
	
	for(int i = 0; i < numVertices; i++)
	{
		vertices[i] = vertices[i] - pinPoint;
		
		float newX = vertices[i].x * cosTheta - vertices[i].y * sinTheta;
		float newY = vertices[i].y * cosTheta + vertices[i].x * sinTheta;
		vertices[i].x = newX;
		vertices[i].y = newY;
		
		vertices[i] = vertices[i] - center;
	}	
	for(int i = 0; i < 4; i++)
	{
		float newX = boltBox[i].x * cosTheta - boltBox[i].y * sinTheta;
		float newY = boltBox[i].y * cosTheta + boltBox[i].x * sinTheta;
		boltBox[i].x = newX;
		boltBox[i].y = newY;
	}
	if(keyPointList)
	{
		keyPointList->Rotate(sinTheta, cosTheta, pinPoint, center);
	}
	
	Vector2D worldPin = pinPoint + position;
	pinPoint = -1.0 * center;
	position = worldPin + center;
	
	float x = upVector.x * cosTheta - upVector.y * sinTheta;
	float y = upVector.y * cosTheta + upVector.x * sinTheta;
	upVector.x = x;
	upVector.y = y;
}
void PinnedObject::AddForce(Vector2D ray)
{
	AddForce(ray, position);
}
void PinnedObject::AddForce(Force force)
{
	AddForce(force.ray, force.origin);
}

void PinnedObject::AddForce(Vector2D ray, Vector2D origin)
{
	Vector2D forcePoint = origin - position;
	if(forcePoint.dist(pinPoint) < 0.001)
		return;
	AddForceObjectSpace(ray, origin - position);
}

void PinnedObject::AddForceObjectSpace(Vector2D ray, Vector2D origin)
{
	if(isPinned)
		return;
	if(ray.Magnitude() == 0.0)
		return;
		
	Vector2D towardPin =  pinPoint - origin;
	Vector2D unitRotate = !towardPin; 
	unitRotate = Vector2D(-unitRotate.y, unitRotate.x);
	Vector2D unitRay = !ray;
		
	float cosTheta = ray.dot(unitRotate);

	Vector2D rayRotational = cosTheta * unitRotate;
	
	torque += rayRotational.x * towardPin.y - rayRotational.y * towardPin.x;
		
	//printf("PinnedObject r = %f\n", towardPin.Magnitude());
	//printf("PinnedObject torque = %f\n", torque);	
}

PinnedObject::PinnedObject()
{
	collideWhenGrabbed = true;
	objectId = PINNEDOBJECT_CLASS;
	position = Vector2D(50,50);
	velocity = Vector2D(0,0);
	acceleration = Vector2D(0,0);
	vertices = (Vector2D*)malloc(sizeof(Vector2D)*4);
	assert(vertices != NULL);
	isPinned = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	isOnGround = false;
	isOnRightWall = false;
	isOnLeftWall = false;
	
	vertices[0] = Vector2D(-1,1);
	vertices[1] = Vector2D(1,1);
	vertices[2] = Vector2D(1,-1);
	vertices[3] = Vector2D(-1,-1);
	pinPoint = Vector2D(0,0);
	CalculateMomentOfInertia();
	//CalculateTextureVertices();
	boltBox[0] = Vector2D(-1,1);
	boltBox[1] = Vector2D(1,1);
	boltBox[2] = Vector2D(1,-1);
	boltBox[3] = Vector2D(-1,-1);
	
}
void PinnedObject::CalculateMomentOfInertia()
{
	 angularMass = 0.0;
	 boundingRadius = 0.0;
	 for(int i = 0; i < numVertices; i++)
	 {
	 	float distance = vertices[i].dist(pinPoint);
	 	angularMass +=  distance * distance;
	 	if(vertices[i].Magnitude() > boundingRadius)
	 		boundingRadius = vertices[i].Magnitude();
	 }
	 angularMass *= mass/numVertices;
}	

PinnedObject::PinnedObject( TextureManager * textureManager,
							Material * material,
				  			float textureRotation,
							Vector2D position, 
							Vector2D velocity,
							Vector2D acceleration,
							Vector2D * vertices,
							int numVertices,
							float rotation,
							float angularVelocity,
							float angularAcceleration,
							float mass,
							bool isPinned,
							Vector2D pinPoint)
{
	printf("PinnedObject::Create\n");
	collideWhenGrabbed = true;
	objectId = PINNEDOBJECT_CLASS;
	this->material = material;
	this->textureManager = textureManager;
	this->position = position;
	this->velocity = velocity;
	this->acceleration = acceleration;
	this->vertices = vertices;
	this->numVertices = numVertices;
	this->rotation = rotation;
	this->angularVelocity = angularVelocity;
	this->angularAcceleration = angularAcceleration;
	this->mass = mass;
	this->isPinned = isPinned;
	this->pinPoint = pinPoint;	
	isOnGround = false;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	CalculateMomentOfInertia();
	printf("PinnedObject::Calculate Texture Vertices\n");
	CalculateTextureVertices(textureRotation);
	
	printf("PinnedObject::LoadTextures\n");
	if(textureManager)
	{
		if(!textureManager->LoadTexture(BOLT_FILE))
			printf("Loading %s failed!", BOLT_FILE);
		//if(!textureManager->LoadTexture(WOOD_FILE))
		//	printf("Loading %s failed!", WOOD_FILE);
	}
	boltBox[0] = Vector2D(-2,2);
	boltBox[1] = Vector2D(2,2);
	boltBox[2] = Vector2D(2,-2);
	boltBox[3] = Vector2D(-2,-2);
	printf("PinnedObject::End\n");
}

PinnedObject::~PinnedObject()
{
	printf("deleting pinned object\n");
	if(vertices)
		delete(vertices);
	vertices = NULL;
	numVertices = 0;
}
void PinnedObject::Draw()
{
	glPushMatrix();
	glLoadIdentity();
	if(textureManager)
	{
			textureManager->EnableTexture(BOLT_FILE);
	}
	Vector2D texCoord[4] = {Vector2D(0,1), Vector2D(1,1), Vector2D(1,0),Vector2D(0,0)};
	glTranslatef(position.x + pinPoint.x,position.y+pinPoint.y, 0.0);
	glEnable(GL_TEXTURE_2D);
	glAlphaFunc( GL_GREATER, 0.5 );
  	glEnable( GL_ALPHA_TEST );
	glBegin(GL_POLYGON);
	for(int i = 0; i < 4; i++)
	{
		glTexCoord2f(texCoord[i].x, texCoord[i].y);
		glVertex2f(boltBox[i].x, boltBox[i].y);
	}
	glEnd();
	
	if(textureManager)
	{
			textureManager->EnableTexture(material->GetTextureIndex());
	}
	glColor3f(1,1,1);
	glTranslatef(-pinPoint.x,-pinPoint.y, 0.0);
	glBegin(GL_POLYGON);
	
	for(int i=0; i < numVertices; i++)
	{
		glTexCoord2f(textureVertices[i].x, textureVertices[i].y);
		glVertex2f(vertices[i].x, vertices[i].y);
	}
	glEnd();
	glDisable( GL_ALPHA_TEST );
	glDisable(GL_TEXTURE_2D);
	
	glPopMatrix();
	
}
int PinnedObject::LineCircleIntersect(Vector2D center, float radius, Vector2D edge[], Vector2D * result)
{
	edge[0] = edge[0] - center;
	edge[1] = edge[1] - center;
	Vector2D deltaEdge = edge[1] - edge[0];
	if(deltaEdge.x == 0 && deltaEdge.y == 0)
		printf("PinnedObject::LineCircleIntersect deltaEdge is (0,0)\n");
		
	float dr2 = deltaEdge.x*deltaEdge.x + deltaEdge.y*deltaEdge.y;
	
	if(dr2 == 0)
		printf("PinnedObject::LineCircleIntersect dr2 == 0\n");
		
	float determinant = edge[0].x * edge[1].y - edge[1].x * edge[0].y;
	float determinant2 = determinant * determinant;
	
	float numx1 = determinant * deltaEdge.y;
	float mult = 1.0;
	if(deltaEdge.y < 0.0)
		mult = -1.0;
	float radius2 = radius*radius;
	float incidence = radius2 * dr2 - determinant2;
	float sqrtPart = sqrt( incidence );
	float numx2 = mult * deltaEdge.x * sqrtPart;
	
	float numy1 = -determinant * deltaEdge.x;
	float numy2 = abs(deltaEdge.y) * sqrtPart;
	
	if(incidence < 0)
		return 0;
		
	result[0] = Vector2D((numx1+numx2)/dr2, (numy1+numy2)/dr2) + center;
	
	if(incidence == 0)
		return 1;
		
	result[1] = Vector2D((numx1-numx2)/dr2, (numy1-numy2)/dr2) + center;
	
	return 2;	
}
float PinnedObject::AngleBetween(Vector2D pointObjectSpace, Vector2D pointWorldSpace)
{
	/*printf("AngleBetween pointObjectSpace= (%f,%f)\n", pointObjectSpace.x, pointObjectSpace.y);
	printf("AngleBetween pointObjectSpace + position = (%f, %f)\n", pointObjectSpace.x+position.x, pointObjectSpace.y+position.y);
	printf("AngleBetween position = (%f, %f)\n", position.x, position.y);
	printf("AngleBetween pointWorldSpace= (%f,%f)\n", pointWorldSpace.x, pointWorldSpace.y);
	printf("AngleBetween pinPoint= (%f,%f)\n",pinPoint.x, pinPoint.y);
	*/
	Vector2D ray1 = pointObjectSpace - pinPoint;
	ray1 = !ray1;	
	Vector2D ray2 = pointWorldSpace - position - pinPoint;
	ray2 = !ray2;
	
	float cosTheta = ray1.dot(ray2);
	float sinTheta = ray1.x*ray2.y - ray1.y*ray2.x;
	
	float mult = 1.0;
	if(sinTheta < 0.0)
		mult = -1.0;
	
	//printf("AngleBetween cosTheta = %f\n", cosTheta);
	float theta = 0;
	if(cosTheta < 1.0)
		theta = acos(cosTheta) * mult;
	//printf("AngleBetween theta = %f\n", theta);
	return theta;
}
void PinnedObject::RotateToEdge(Vector2D vertex, Vector2D edge[])
{
	RotateToEdge(vertex,edge,true);
}
void PinnedObject::RotateToEdge(Vector2D vertex, Vector2D edge[], bool changeVelocity)
{
	float radius = pinPoint.dist(vertex);
	//printf("RotateToEdge radius = %f\n", radius);
	Vector2D intersections[2];
	int numIntersects = LineCircleIntersect(pinPoint + position, radius, edge, intersections);
	if(numIntersects > 0)
	{
		float theta = AngleBetween(vertex, intersections[0]);
		//printf("RotateToEdge intersections0 = (%f,%f)\n",intersections[0].x, intersections[0].y);
		//printf("RotateToEdge theta = %f\n",theta);
		if(numIntersects > 1)
		{
			float theta2 = AngleBetween(vertex, intersections[1]);
			//printf("RotateToEdge intersections1 = (%f,%f)\n",intersections[1].x, intersections[1].y);
			//printf("RotateToEdge theta2 = %f\n",theta2);
			if(abs(theta2) < abs(theta))
				theta = theta2;
		}

		if(changeVelocity && theta != 0.0)
		{
			angularVelocity += theta/lastElapsedTime;
		}
		Rotate(theta);
		
	}
}
void PinnedObject::RotateToVertex(Vector2D vertex, Vector2D edge[])
{
	RotateToVertex(vertex,edge,false);
}
void PinnedObject::RotateToVertex(Vector2D vertex, Vector2D edge[], bool changeVelocity)
{
	float radius = pinPoint.dist(vertex);
	Vector2D intersections[2];
	int numIntersects = LineCircleIntersect(pinPoint + position, radius, edge, intersections);
	if(numIntersects > 0)
	{
		float theta = AngleBetween(vertex, intersections[0]);
		if(numIntersects > 1)
		{
			float theta2 = AngleBetween(vertex, intersections[1]);
			if(abs(theta2) < abs(theta))
				theta = theta2;
		}
		
		if(changeVelocity && theta != 0.0)
		{
			angularVelocity -= theta/lastElapsedTime;
		}
		Rotate(-theta);
		
	}
}
